
#include "KeyTranslator.h"
#include "PS2Keyboard.h"
#include <string.h>
#include <stdlib.h>


static void* keyboardID[MAX_KEYBOARDS];
static uint16_t keyboardSize[MAX_KEYBOARDS];
static KeyAction translatedKeyAction[MAX_KEYBOARDS];
static CommandResponse commandResult[MAX_KEYBOARDS];
static uint8_t matrixData[TRANSLATION_SIZE];
static uint8_t matrixIDIndex = 0xFF;
static uint8_t matrixDataIndex = 0;
static void* matrixAddress = 0;

bool CheckValidArrayIndex(uint8_t id){
    if (id < 1 || id > MAX_KEYBOARDS) return false;
    return true;
}

void TranslatorInit(){
    uint32_t size = (uint32_t) sizeof(keyboardID);
    memset(keyboardID, 0, size);
    size = (uint32_t) sizeof(translatedKeyAction);
    memset(translatedKeyAction, 0, size);
    size = (uint32_t) sizeof(commandResult);
    memset(commandResult, 0, size);
    size = (uint32_t) sizeof(keyboardSize);
    memset(keyboardSize, 0, size);
}

// Allocate space for 1 keyboard.  May have many.  Returns ID of keyboard.
// Returns 0 if no space available.
uint8_t AllocateTranslator(uint8_t size){
    uint8_t index;
    for (index = 0; index < MAX_KEYBOARDS; index++){
        if (keyboardID[index] == 0){
            keyboardID[index] = malloc(size * TRANSLATION_SIZE);
            if (keyboardID[index] == 0) return 0;
            memset(keyboardID[index], 0, size * TRANSLATION_SIZE);
            keyboardSize[index] = size;
            return index + 1;
        }
    }
    return 0;
}

// Releases all translators
void DeallocateTranslators(){
    uint8_t index;
    for (index = 0; index < MAX_KEYBOARDS; index++){
        if (keyboardID[index] != 0){
            free(keyboardID[index]);
            translatedKeyAction[index] = 0;
            commandResult[index] = 0;
            keyboardSize[index] = 0;
            keyboardID[index] = 0;
        }
    }
}

// Release a single translator
void DeallocateTranslator(uint8_t id){
    if (CheckValidArrayIndex(id)){
        uint8_t index = id - 1;
        free(keyboardID[index]);
        translatedKeyAction[index] = 0;
        commandResult[index] = 0;
        keyboardSize[index] = 0;
        keyboardID[index] = 0;
    }
}

// Callbacks for a particular translator.
void TranslatorCallback(uint8_t id, KeyAction keyAction, CommandResponse commandResponse){
    if (CheckValidArrayIndex(id)){
        translatedKeyAction[id - 1] = keyAction;
        commandResult[id - 1] = commandResponse;
    }
}

void AddConversion(uint8_t id, uint16_t translation, uint8_t codes[BYTES_PER_KEY]){
    if (CheckValidArrayIndex(id)){
        uint16_t index1;
        uint8_t index2;
        void* address = keyboardID[id - 1];
        for (index1 = 0; index1 < keyboardSize[id-1]; index1++){
            uint16_t data = *((uint8_t*)(address + (index1 * TRANSLATION_SIZE)));
            if (data == 0 || data == translation){
                *(uint16_t*)(address + (index1 * TRANSLATION_SIZE)) = translation;
                for (index2 = 0; index2 < BYTES_PER_KEY; index2++){
                    *((uint8_t*)(address + 2 + index2 + (index1 * TRANSLATION_SIZE))) = codes[index2];
                }
                break;
            }
        }
    }
}

void ResetConversions(uint8_t id){
    if (CheckValidArrayIndex(id)){
        if (keyboardID[id - 1] != 0){
            memset(keyboardID[id - 1], 0, keyboardSize[id - 1]);
        }
    }
}

// Used for PS2 data
void CreateTranlatedData(uint8_t* codes, uint8_t press1, uint8_t press2, uint8_t press3, uint8_t release1, uint8_t release2, uint8_t release3){
    codes[0] = press1;
    codes[1] = press2;
    codes[2] = press3;
    codes[3] = release1;
    codes[4] = release2;
    codes[5] = release3;
}
// Used for USB data
void CreateTranslatedData(uint8_t* codes, uint8_t press1, uint8_t release1){
    codes[0] = press1;
    codes[1] = USB_ID;
    codes[2] = USB_ID;
    codes[3] = release1;
    codes[4] = USB_ID;
    codes[5] = USB_ID;
}

// The raw data from keyboard to translate
void Translate(uint8_t id, uint8_t code1, uint8_t code2, uint8_t code3){
    if (CheckValidArrayIndex(id)){
        bool command = false;
        // First see if this is a command.  If so just forward the command.
        switch (code1){
            case PS2K_RESET:
            case PS2K_ECHO:
            case PS2K_LED:
            case PS2K_DEFAULTS:
            case PS2K_ENABLE_SCAN:
            case PS2K_DISABLE_SCAN:
            case PS2K_IDENTIFY:
            case PS2K_TYPE_RATE:
                command = true;
                break;
            case PS2K_SCAN_CODE_SET:
                // Could be scan code set or release key
                if (code3 != 0){
                    command = true;
                }
                break;
            default:
                break;
        }
        if (command){
            // Check if anyone cares about the command
            if (commandResult[id - 1] != 0){
                bool success = false;
                if (code2 == PS2K_ACK) success = true;
                commandResult[id - 1](code1,success,code3);
            }
        } else {
            uint16_t index;
            uint8_t* raw_data;
            void* address = keyboardID[id - 1];
            for (index = 0; index < keyboardSize[id-1]; index++){
                raw_data = (uint8_t*)address + (index * TRANSLATION_SIZE);
                if (*(raw_data + 2) == code1){    //Press
                    if (*(raw_data + 3) == code2 && *(raw_data + 4) == code3){
                        // We found a key
                        if (translatedKeyAction[id - 1] != 0){
                            translatedKeyAction[id - 1](KEY_PRESS | *((uint16_t*)(raw_data)));
                        }
                        break;
                    }
                }

                if (*(raw_data + 5) == code1){    //Press
                    if (*(raw_data + 6) == code2 && *(raw_data + 7) == code3){
                        // We found a key
                        if (translatedKeyAction[id - 1] != 0){
                            translatedKeyAction[id - 1](KEY_RELEASE | *((uint16_t*)(raw_data)));
                        }
                        break;
                    }
                }
            }
        }
    }
}

// Start a matrix download
uint8_t BeginKeyTranslationMatrix(uint8_t id, uint8_t size){
    if (CheckValidArrayIndex(id)){
        matrixDataIndex = 0;
        DeallocateTranslator(id);
        keyboardID[id - 1] = malloc(size * TRANSLATION_SIZE);
        if (keyboardID[id - 1] == 0) return 0;
        memset(keyboardID[id - 1], 0, size * TRANSLATION_SIZE);
        keyboardSize[id - 1] = size;
        matrixIDIndex = id - 1;
        matrixAddress = keyboardID[id - 1];
        return id;
    }
}
// Convert incoming bytes into a key translation matrix.
// Data is high byte of the 16 bit code then low byte of the 16 bit code.
// Then comes the 6 bytes from the codes array starting with index 0 and
// ending with index 5.
bool AddKeyTranslationMatrix(uint8_t data){
    if (matrixDataIndex != 0xFF){
        matrixData[matrixDataIndex++] = data;
        if (matrixDataIndex == TRANSLATION_SIZE){
            *((uint8_t*)(matrixAddress + 0)) = matrixData[1];
            *((uint8_t*)(matrixAddress + 1)) = matrixData[0];
            *((uint8_t*)(matrixAddress + 2)) = matrixData[2];
            *((uint8_t*)(matrixAddress + 3)) = matrixData[3];
            *((uint8_t*)(matrixAddress + 4)) = matrixData[4];
            *((uint8_t*)(matrixAddress + 5)) = matrixData[5];
            *((uint8_t*)(matrixAddress + 6)) = matrixData[6];
            *((uint8_t*)(matrixAddress + 7)) = matrixData[7];
            matrixAddress += TRANSLATION_SIZE;
            matrixDataIndex = 0;
        }
    }
}
// Abort the creation and clean up everything done so far.
void AbortCreateKeyTranslationMatrix(){
    if (CheckValidArrayIndex(matrixDataIndex + 1)){
        DeallocateTranslator(matrixIDIndex + 1);
        matrixDataIndex = 0xFF;
        matrixAddress = 0;
        matrixDataIndex = 0;
    }
}